#!/bin/bash
### BEGIN INIT INFO
# Provides:          rtnet_kuka
# Required-Start:    $local_fs
# Required-Stop:     $local_fs
# Default-Start:     2 3 4 5
# Default-Stop:      0 1 6
# X-Interactive:     false
# Short-Description: Starts RTNet components for the Kuka LWR
# Description:       Start/stop RTNet components for the Kuka LWR 4+ at ISIR
### END INIT INFO

VERT="\\033[1;32m"
NORMAL="\\033[0;39m"
NC='\033[0m'
ROUGE="\\033[1;31m"
ROSE="\\033[1;35m"
BLEU="\\033[1;34m"
BLANC="\\033[0;02m"
BLANCLAIR="\\033[1;08m"
JAUNE="\\033[1;33m"
CYAN="\\033[1;36m"

SLAVES="192.168.100.102 192.168.100.103"
SLAVES_NAMES="Kuka ATISensor"
### Xenomai 2+RTNET
prefix="/usr/local/rtnet"
prefix_bin="${prefix}/sbin"
modules_dir="modules"
RTNETCFG="${prefix}/etc/rtnet.conf"

bringup_slave()
{
        slave=$1
        slave_name=$2
        slave_number=$3
        sleep_time_s=1.0
        max_trials=10
        rc=0
        count=$max_trials                   # Maximum number to try.
        while [ $count -ne 0 ] ; do
        	  sudo ${prefix_bin}/rtroute solicit $slave dev rteth$slave_number
            sudo ${prefix_bin}/rtping -c 1 $slave              # Try once.
            rc=$?
            if [ $rc -eq 0 ] ; then
                count=1                      # If okay, flag to exit loop.
            else
                echo -e "$BLANC" "Remaining trials $count for $slave_name ${NC}"
            fi
            count=$((count - 1))                  # So we don't go forever.
            sleep $sleep_time_s
        done

        if [ $rc -eq 0 ] ; then                  # Make final determination.
            echo -e "$VERT" "$slave_name is connected. ${NC}"
        else
            echo -e "$ROUGE" "$slave_name timed out. Please start again if $slave_name is plugged in. ${NC}"
        fi
}

do_start()
{
   echo "Starting RealTime Network";

   # Read the config file
   if [ -r $RTNETCFG ]; then
       . $RTNETCFG
   else
       echo -e "$ROUGE" "Could not read $RTNETCFG ${NC}"
       exit 1
   fi

   sudo modprobe -r ${RT_DRIVER#$"rt_"} 
   #cd ${prefix}/sbin/
   sudo ${prefix_bin}/rtnet start
   sleep 0.5

   # Bring up connection
   #i=0
   #rc=0
   #for SLAVE in $SLAVES; do
   #		sudo ${prefix_bin}/rtifconfig rteth$i up $IPADDR netmask $NETMASK
   #		#${prefix_bin}/rtroute solicit $SLAVE dev rteth$i
  # 		i=$((i+1))
   #	done

   # Wait until connection is good for all slaves
   i=0
   names=($SLAVES_NAMES)
   for SLAVE in $SLAVES; do
   		   #echo "${prefix_bin}/rtroute solicit $SLAVE dev rteth$i"
           sudo ${prefix_bin}/rtifconfig rteth$i up $IPADDR netmask $NETMASK
           bringup_slave $SLAVE ${names[$i]} $i &
           i=$((i+1))
   done

}

do_stop()
{
  echo "Stoping RealTime Network";
  cd ${prefix}/sbin/
  sudo ${prefix_bin}/rtnet stop
}


case "$1" in
   start)
     do_start
     ;;
   stop)
     do_stop
     ;;
   restart|reload|condrestart)
     do_stop
     do_start
     ;;
*)
     echo $"Usage: $0 {start|stop|restart|reload|status}"
     exit 1
esac
exit 0
